from utils.utm_wgs84 import LLtoUTM


def xyConversionlatAndLon(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    longitude, latitude = rule1(pos[0] - offset[0], pos[1] - offset[1], inverse=True)
    return longitude, latitude


# 经纬度转换xy
def latAndLonConversionXY(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    x1, y1 = rule1(pos['lon'], pos['lat'])

    x = x1 + offset[0]
    y = y1 + offset[1]
    return x, y


import math


def calculate_rear_axle_coordinates(center_lat, center_lon, heading_angle, vehicle_length):
    # Convert latitude and longitude to radians
    center_lat_rad = math.radians(center_lat)
    center_lon_rad = math.radians(center_lon)

    # Earth's radius in meters
    R = 6371000

    # Convert vehicle length from meters to radians (assuming small angles)
    vehicle_length_rad = vehicle_length / R

    # Convert heading angle to radians
    heading_angle_rad = math.radians(heading_angle)

    # Calculate offsets for rear axle in local coordinate system
    dx = (vehicle_length_rad / 2) * math.cos(heading_angle_rad)
    dy = (vehicle_length_rad / 2) * math.sin(heading_angle_rad)

    # Convert offsets from radians to meters
    dx_meters = dx * R
    dy_meters = dy * R

    # Calculate rear axle coordinates
    rear_axle_lat = center_lat + (dy_meters / R) * (180 / math.pi)
    rear_axle_lon = center_lon + (dx_meters / (R * math.cos(center_lat_rad))) * (180 / math.pi)

    return rear_axle_lon, rear_axle_lat


def calculate_vehicle_corners(center_lon, center_lat, heading, length, width):
    """
    计算车辆四个角点的经纬度
    :param center_lon: 车辆中心点的经度
    :param center_lat: 车辆中心点的纬度
    :param heading: 车辆的航向角（以度为单位，正北方向为0度，顺时针增加）
    :param length: 车辆的长度（单位：米）
    :param width: 车辆的宽度（单位：米）
    :return: 四个角点的经纬度坐标，按顺时针顺序返回（前左，前右，后右，后左）
    """
    # 将航向角转换为弧度
    heading_rad = math.radians(heading)

    # 将长度和宽度转换为半长度和半宽度
    half_length = length / 2.0
    half_width = width / 2.0

    # 计算车辆四个角点的相对位置（相对于车辆中心点）
    front_left_offset = (-half_width, half_length)
    front_right_offset = (half_width, half_length)
    rear_right_offset = (half_width, -half_length)
    rear_left_offset = (-half_width, -half_length)

    # 计算四个角点的经纬度
    front_left_lat = center_lat + (
            front_left_offset[0] * math.cos(heading_rad) - front_left_offset[1] * math.sin(heading_rad)) / 111111
    front_left_lon = center_lon + (
            front_left_offset[0] * math.sin(heading_rad) + front_left_offset[1] * math.cos(heading_rad)) / (
                             111111 * math.cos(center_lat))

    front_right_lat = center_lat + (
            front_right_offset[0] * math.cos(heading_rad) - front_right_offset[1] * math.sin(heading_rad)) / 111111
    front_right_lon = center_lon + (
            front_right_offset[0] * math.sin(heading_rad) + front_right_offset[1] * math.cos(heading_rad)) / (
                              111111 * math.cos(center_lat))

    rear_right_lat = center_lat + (
            rear_right_offset[0] * math.cos(heading_rad) - rear_right_offset[1] * math.sin(heading_rad)) / 111111
    rear_right_lon = center_lon + (
            rear_right_offset[0] * math.sin(heading_rad) + rear_right_offset[1] * math.cos(heading_rad)) / (
                             111111 * math.cos(center_lat))

    rear_left_lat = center_lat + (
            rear_left_offset[0] * math.cos(heading_rad) - rear_left_offset[1] * math.sin(heading_rad)) / 111111
    rear_left_lon = center_lon + (
            rear_left_offset[0] * math.sin(heading_rad) + rear_left_offset[1] * math.cos(heading_rad)) / (
                            111111 * math.cos(center_lat))

    return [(front_left_lon, front_left_lat), (front_right_lon, front_right_lat), (rear_right_lon, rear_right_lat),
            (rear_left_lon, rear_left_lat)]


# 左前角 (corner1)
# 右前角 (corner2)
# 右后角 (corner3)
# 左后角 (corner4)
def calculate_vehicle_corner_coordinates(center_lat, center_lon, heading_angle, vehicle_length, vehicle_width):
    # Convert latitude and longitude to radians
    center_lat_rad = math.radians(center_lat)
    center_lon_rad = math.radians(center_lon)

    # Earth's radius in meters
    R = 6371000

    # Convert vehicle dimensions from meters to radians (assuming small angles)
    half_length_rad = vehicle_length / (2 * R)
    half_width_rad = vehicle_width / (2 * R)

    # Convert heading angle to radians
    heading_angle_rad = math.radians(heading_angle)

    # Calculate offsets for vehicle corners in local coordinate system
    dx1 = half_length_rad * math.cos(heading_angle_rad) + half_width_rad * math.sin(heading_angle_rad)
    dy1 = half_width_rad * math.cos(heading_angle_rad) - half_length_rad * math.sin(heading_angle_rad)
    dx2 = half_length_rad * math.cos(heading_angle_rad) - half_width_rad * math.sin(heading_angle_rad)
    dy2 = -half_width_rad * math.cos(heading_angle_rad) - half_length_rad * math.sin(heading_angle_rad)
    dx3 = -half_length_rad * math.cos(heading_angle_rad) - half_width_rad * math.sin(heading_angle_rad)
    dy3 = -half_width_rad * math.cos(heading_angle_rad) + half_length_rad * math.sin(heading_angle_rad)
    dx4 = -half_length_rad * math.cos(heading_angle_rad) + half_width_rad * math.sin(heading_angle_rad)
    dy4 = half_width_rad * math.cos(heading_angle_rad) + half_length_rad * math.sin(heading_angle_rad)

    # Convert offsets from radians to meters
    dx1_meters = dx1 * R
    dy1_meters = dy1 * R
    dx2_meters = dx2 * R
    dy2_meters = dy2 * R
    dx3_meters = dx3 * R
    dy3_meters = dy3 * R
    dx4_meters = dx4 * R
    dy4_meters = dy4 * R

    # Calculate vehicle corner coordinates
    corner1_lat = center_lat + (dy1_meters / R) * (180 / math.pi)
    corner1_lon = center_lon + (dx1_meters / (R * math.cos(center_lat_rad))) * (180 / math.pi)
    corner2_lat = center_lat + (dy2_meters / R) * (180 / math.pi)
    corner2_lon = center_lon + (dx2_meters / (R * math.cos(center_lat_rad))) * (180 / math.pi)
    corner3_lat = center_lat + (dy3_meters / R) * (180 / math.pi)
    corner3_lon = center_lon + (dx3_meters / (R * math.cos(center_lat_rad))) * (180 / math.pi)
    corner4_lat = center_lat + (dy4_meters / R) * (180 / math.pi)
    corner4_lon = center_lon + (dx4_meters / (R * math.cos(center_lat_rad))) * (180 / math.pi)
    return (corner1_lon, corner1_lat), (corner2_lon, corner2_lat), (corner3_lon, corner3_lat), (
        corner4_lon, corner4_lat)


import math


def calculate_midpoint(lat1, lon1, lat2, lon2):
    # Convert latitude and longitude to radians
    lat1_rad = math.radians(lat1)
    lon1_rad = math.radians(lon1)
    lat2_rad = math.radians(lat2)
    lon2_rad = math.radians(lon2)

    # Earth's radius in meters
    R = 6371000

    # Convert latitude and longitude to Cartesian coordinates
    x1 = R * math.cos(lat1_rad) * math.cos(lon1_rad)
    y1 = R * math.cos(lat1_rad) * math.sin(lon1_rad)
    z1 = R * math.sin(lat1_rad)

    x2 = R * math.cos(lat2_rad) * math.cos(lon2_rad)
    y2 = R * math.cos(lat2_rad) * math.sin(lon2_rad)
    z2 = R * math.sin(lat2_rad)

    # Calculate midpoint coordinates in Cartesian system
    x_mid = (x1 + x2) / 2
    y_mid = (y1 + y2) / 2
    z_mid = (z1 + z2) / 2

    # Convert midpoint coordinates back to latitude and longitude
    mid_lat_rad = math.atan2(z_mid, math.sqrt(x_mid ** 2 + y_mid ** 2))
    mid_lon_rad = math.atan2(y_mid, x_mid)

    mid_lat = math.degrees(mid_lat_rad)
    mid_lon = math.degrees(mid_lon_rad)

    return mid_lat, mid_lon


import math


def calculate_easting_northing_velocity(speed, yaw):
    # 将航向角转换为弧度
    yaw_rad = math.radians(yaw)

    # 计算速度的东向分量和北向分量
    easting_velocity = speed * math.cos(yaw_rad)
    northing_velocity = speed * math.sin(yaw_rad)

    return easting_velocity, northing_velocity


def calculate_utm(x, y, x0, y0, angle_degrees):
    # 角度转换为弧度
    angle_radians = math.radians(angle_degrees)

    # 计算坐标差
    dx = x - x0
    dy = y - y0

    # 计算旋转矩阵
    cos_a = math.cos(angle_radians)
    sin_a = math.sin(angle_radians)

    # 计算旋转后的坐标差
    dx_prime = cos_a * dx - sin_a * dy
    dy_prime = sin_a * dx + cos_a * dy

    # 计算相对于(x0, y0)的相对坐标
    # x_relative = dx_prime + x0
    # y_relative = dy_prime + y0
    x_relative = dx_prime
    y_relative = dy_prime
    return x_relative, y_relative
